def connect_wifi():
    import network
    sta_if = network.WLAN(network.STA_IF)
    if not sta_if.isconnected():
        print('connecting to network...')
        sta_if.active(True)
        sta_if.connect('ChinaNet-62Rd', 'DMJe7mtS')
        while not sta_if.isconnected():
            pass
    print('network config:', sta_if.ifconfig())

connect_wifi()

#import webrepl
#webrepl.start()

from mpu6050 import Mpu6050
import time
import socket,asyncio

mpu6050 = Mpu6050()

buf=bytearray(14)
frame = bytearray(19)
frame[0]=0x98


def get_and_send(s):
    while True:
        start = time.time()
        mpu6050.read_6_axis_data(buf)
        end = time.time()
        
        dt = end - start
        
        for index, singleByte in enumerate(buf):
        
            frame[index+1] = singleByte
        
        frame[15] = (dt & 0xff000000) >> 24
        frame[16] = (dt & 0xff0000) >> 16
        frame[17] = (dt & 0xff00) >> 8
        frame[18] = (dt & 0xff) 
        
        s.send(frame)


def transport_mpu6050_via_tcp():
    
    while True:
        try:
            s = socket.socket()
            s.connect(('192.168.1.140',39999))
            print("success to connect server")
            
            get_and_send(s)

            #data = s.recv(1000)
            s.close()
        except  Exception as e:
            print("connection exception, retrying...  ")
            time.sleep(1)
            #raise e
        


asyncio.run(transport_mpu6050_via_tcp())
